COMMENT ⊗   VALID 00014 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002	.TITLE DIAG
C00008 00003	PROGRAM INITIALIZATION 
C00011 00004	SETUP COEFFICIENT DATA LIST
C00017 00005	ROUTINE TO DISPLAY CURRENT JOINT ANGLES
C00021 00006	ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS
C00023 00007	ROUTINE TO CHECK THE TOUCH SENSOR ON MONITOR SUBROUTINES 
C00025 00008	CENTER FUNCTION TEST ROUTINE
C00027 00009	TACHOMETER CALIBRATION ROUTINE
C00030 00010	ROUTINE TO TEST READ BASE VALUES FROM WRIST
C00032 00011	ROUTINE TO READ AND RESOLVE WRIST FORCES
C00034 00012	ROUTINES TO READ A/D CHANNELS
C00039 00013	DRIVE COEFFICIENT DATA LIST
C00041 00014	LOCAL STORAGE AREA
C00045 ENDMK
C⊗;
.TITLE DIAG

TRANS ==1	;SET TO 1 TO PRINT OUT EULER AS WELL AS JOINT ANGLES

SNGSTP==0	;CONDITIONAL ASSEMBLY FLAGS FOR ARM.PAL
DIAGY ==1
STDALN==1	;USE ARM CODE IN STAND ALONE VERSION
TACCAL==0
NOYELW==0
TIMER ==0
ISLIN ==1
SHORT==0
FRCDAT==0	
FFING==1	;FORCE SENSING FINGERS 
CPOINTY==0	;NOT A POINTY COMPILATION

.OFFSET -160000		;Load us into the high mem: virtual 0 → physical 160 000

.IF1
  .INSRT K1DEF.PAL[11,SYS]
  .INSRT ALHEAD.PAL[AL,HE]
   STSW  KERNEL,1	;1 => use the kernel
.ENDC

.OFFSET -340000		;Load us into higher mem: virtual 0 → physical 340 000

.=1000			;RESTART KERNEL IF IN USER SPACE
RESTRT

.= ARMCOD
CODE$ == armcod		;arm code & data spaces start here
DATA$ == armdat
SPSWITCH == 0		;Make sure we start off with everyone properly defined

DATA
.INSRT ARM.pal[AL,HE]
.INSRT BEJCZY.PAL[AL,HE]
.INSRT ARITH.PAL[AL,HE]
.INSRT ALIO.PAL[AL,HE]
.INSRT ARMSOL.PAL[AL,HE]
.INSRT FLOAT.PAL[AL,HE]
.IFNZ TRANS
   .INSRT EULER.PAL[AL,HE]
.ENDC

;COMMUNICATION LINK FOR "DIAGY"

CMDARR==COMTAB		;PHYSICAL ADDRESS = COMTAB+160000
JOINT ==CMDARR+2	;JOINT NUMBER TO BE DRIVEN
TIME  ==CMDARR+4	;DURATION OF DRIVE MOTION
JTANG ==CMDARR+6	;CHANGE IN JOINT ANGLE, F.P.
MASLOC==CMDARR+12	;ADDRESS OF MASTER CHECK NUMBER FROM PDP10
DSTAT ==CMDARR+14	;FUNCTION STATUS

MASTER==13131	;CHECK NUMBER FROM PDP10


;COMMUNICATION LINK FOR "TLKEF5", "WHERE" ROUTINE FOR THE PDP10

MASLC5==CMDARR+100 ;ADDRESS OF MASTER NUMBER TO SIGNAL PDP10 WE ARE ALIVE
TRNADR==MASLC5+2   ;SAVE TRANSFORM DATA IN HERE
JTSADR==MASLC5+102 ;SAVE JOINT ANGLES IN HERE

MASTR5==12121	;CHECK NUMBER TO PDP10


;DEVICE MECHANISM BITS

YELARM== 1	;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
YELHND== 2 	;YELLOW HAND
BLUARM== 4	;BLUE ARM, NOT INCLUDING HAND
BLUHND== 10	;BLUE HAND
VISE==20	;VISE *K
;PROGRAM INITIALIZATION 

.EVEN
CODE

.MACRO	TAB
MOV	#11,R0
JSR	PC,TYPCHR
.ENDM

.MACRO	SPACE
MOV	#40,R0
JSR	PC,TYPCHR
.ENDM

.MACRO	CR
MOV	#15,R0		;PUT IN A CR
JSR	PC,TYPCHR
.ENDM

DINIT:	JSR	PC,IOINIT	;SET UP IO STUFF
	BPT
	BPT

DIAG:	MOV	#DEVICE,R1	;INITIALIZE ARMS FOR MOVING
	JSR	PC,INTARM
	TST	R0		;CHECK IF ANY ERRORS OCCURRED
	BEQ	STRTOK		;BRANCH IF EVERYTHING OK
BADINT:	MOV	R0,-(SP)	;SAVE ERROR #
	MOV	#BADSTR,R0	;ELSE TYPE ERROR MESSAGE
TYPERR:	JSR	PC,TYPSTR
	MOV	(SP)+,R0	;TYPE ERROR CODE
	JSR	PC,TYPOCT
	CRLF
	BPT
STRTOK:	CLR	MASLOC		;CLEAR MASTER NUMBER FROM PDP10
     	MOV 	#-1,CMDARR	;INDICATE READY TO RECEIVE COMMAND BLOCK FROM 10

;WAIT LOOP LOOKING AT COMMAND BLOCK FROM PDP10

WTLP: 	JSR	PC,KBDCHK	;SETS CARRY BIT IF KBD HIT
	BCC	CHKMST
	CLR	CMDARR		;INDICATE DIAG NO LONGER ACTIVE
	BPT			;EXIT TO DDT

CHKMST:	TST	MASLOC		;CHECK IF COMMAND BLOCK RECEIVED FROM PDP10
	BEQ	WTLP		;GO LOOK AGAIN IF ZERO    
	CMP	#MASTER,MASLOC	;CHECK IF RIGHT MASTER NUMBER FROM PDP10
	BEQ	SMENUM
	MOV	#-2,CMDARR	;SET ERROR CODE INDICATING BAD NUMBER
	CLR	MASLOC		;CLEAR THE BAD MASTER NUMBER
	JMP	WTLP		;GO WAIT FOR ANOTHER COMMAND BLOCK
SMENUM:	CLR	MASLOC 

;SETUP COEFFICIENT DATA LIST

	MOV	#100000,R3	;THIS IS YELLOW ARM JOINT 1 SERVO BIT
	TST	CMDARR     	;GET THE ARM NUMBER, EQUAL TO 1 IF BLUE ARM
	BEQ	.+6   		;SKIP IF YELLOW
	MOV	#400,R3		;THIS IS BLUE ARM JOINT 1 SERVO BIT
	MOV	JOINT,R1	;GET THE JOINT NUMBER 1-7
	DEC	R1     
       	NEG	R1
	ASHC	R1,R3		;ROTATE BIT IN R3 TO INDICATE PROPER JOINT 

       	MOV	R3,DLST		;SET SERVO BIT
	MOV	TIME,DTIME	;SET DURATION OF DRIVE MOTION
	MOV	#DA0,R0		;SET A0,A1,A2 EQUAL TO ZERO
	MOV	#6,R1
	CLR	(R0)+
	SOB	R1,.-2
	LDF	JTANG,AC0	;GET THE CHANGE IN JOINT ANGLE
	LDF	C10,AC1		;SCALE 5TH ORDER POLYNOMIAL BY CHANGE AND PACK
	MULF	AC0,AC1		;  IN TO COEFFICIENT DATA LIST
	STF	AC1,DA3
	LDF	CM15,AC1
	MULF	AC0,AC1
	STF	AC1,DA4
	LDF	C6,AC1
	MULF	AC0,AC1
	STF	AC1,DA5

;SET UP THE DYNAMIC COEFFICIENT LIST CI AND CII

	BIT	#YARM+YHAND,DLST	;CHECK IF THIS IS FOR THE YELLOW ARM
	BEQ	NOYEL
	MOV	#YARM+YHAND,WLST	;IF SO, READ THE CURRENT JOINT ANGLES
	MOV	#YELARM,R2	;INDICATE YELLOW ARM
	MOV	#YTH,R4		;GET POINTER TO YELLOW JOINT ANGLES
	BR	FILLAN
NOYEL:	BIT	#BARM+BHAND,DLST	;CHECK IF THIS FOR THE BLUE ARM
	BEQ	NOBLU
	MOV	#BARM+BHAND,WLST	;ELSE MUST BE BLUE ARM
	MOV	#BLUARM,R2	;INDICATE BLUE ARM
	MOV	#BTH,R4		;GET POINTER TO BLUE JOINT ANGLES
	BR 	FILLAN
NOBLU:	BIT	#VISE,DLST	;CHECK IF THIS IS FOR VISE
	BEQ	1$
	MOV	#VISE,WLST
	MOV	#VIS,R2		;INDICATE VISE
	MOV	#HTH,R4		;GET POINTER TO HARDWARE POSITIONS
	BR	FILLAN
1$:	HALT			;SHOULDNT BE HERE UNTIL SCREWDRIVER EXISTS

FILLAN:	MOV	#WLST,R0
	MOV	#DEVICE,R1
	JSR	PC,WHERE
	BIT	#YHAND+BHAND+VISE,DLST ;CHECK IF HAND|VISE SELECTED FOR DRIVE
	BNE	ISHND		;BRANCH IF HAND OPERATION
	MOV	JOINT,R3	;GET POINTER TO JOINT ANGLE
	DEC	R3
	ASH	#1,R3
	ADD	R4,R3
	LDF	@(R3),AC0	;GET CURRENT JOINT ANGLE
	ADDF	JTANG,AC0	;COMPUTE FINAL JOINT ANGLE
	STF	AC0,@(R3)
	MOV	R4,R0		;COMPUTE CORRESPONDING ARM DYNAMIC COEFFICIENTS
	MOV	#CIPTR,R1
	JSR	PC,DTERMS
	MOV	JOINT,R3	;GET INDEX TO DYNAMIC COEFFICIENTS
	ASH	#2,R3
	LDF	CILST-4(R3),AC0	;PICK UP GRAVITY LOADING AND INERTIA
	STF	AC0,SEGCI
	LDF	CIILST-4(R3),AC0
	STF	AC0,SEGCII
       	MOV	#WLST,R0	;RESTORE CURRENT JOINT ANGLES
	MOV	#DEVICE,R1
	JSR	PC,WHERE
	BR	DODRVE
ISHND:
	BIT	#YHAND,DLST	;CHECK IF YELLOW HAND
	BEQ	NOYHND
	LDF	CI+SRV07,AC0	;IF YELLOW GET PROPER CI AND CII
	LDF	CII+SRV07,AC1
	BR	DOHAND
NOYHND:	BIT	#BHAND,DLST	;CHECK IF BLUE HAND
	BEQ	NOBHND
	LDF	CI+SRV14,AC0	;IF BLUE GET PROPER CI AND CII
	LDF	CII+SRV14,AC1
 	BR	DOHAND
NOBHND:	BIT	#VISE,DLST	;CHECK IF VISE
	BEQ	NOVISE
	LDF	CI+SRV15,AC0	;IF VISE GET PROPER CI AND CII
	LDF	CII+SRV15,AC1
	BR 	DOHAND
NOVISE:	MOV	#NSCR,SG	;SAY LOOKING FOR SCREWDRIVER
	JSR	PC,TYPSTR
	BPT
DOHAND:	STF	AC0,SEGCI	;PUT DYNAMIC COEF. IN DATA LIST
	STF	AC1,SEGCII

;START UP ARM MOTION USING DRIVE FUNCTION

DODRVE: MOV	#DLST,R0	;POINT TO COEFFICIENTS
	MOV	#DEVICE,R1	;POINT TO DEVICE BLOCK
	JSR	PC,DRIVE
	MOV	R0,DSTAT	;SAVE FUNCTION STATUS

;INDICATE EXECUTION COMPLETED TO PDP10

RET:	MOV	DBUF,R0      	;SET DBUF EQUAL TO THE NUMBER OF DATA BLOCKS
	SUB	#DBUF+2,R0	;   COLLECTED
	ASR	R0
	MOV	R0,DBUF
     	MOV	#DBUF,R0	;GET THE ADDRESS OF THE DYNAMIC RESPONSE DATA
	ASR	R0		;GET WORD ADDRESS
	NEG	R0		;COMPLEMENT TO USE AS FLAG TO TELL PDP10 WERE DONE
	MOV	R0,CMDARR
	JMP	WTLP		;GO LOOP WAITING FOR ANOTHER COMMAND BLOCK


;END OF EXECUTABLE CODE
;ROUTINE TO DISPLAY CURRENT JOINT ANGLES

YW:	MOV	#YARM+YHAND+2,WLST	;DO WHERE FOR YEL ARM + VISE
	CLR	WSWCH
	BR	WSTRT
BW:
W:	MOV	#BARM+BHAND+2,WLST	;DO WHERE FOR BLUE ARM + VISE
	MOV	#16,WSWCH

WSTRT:	OUTSTR	TOPCLR
	OUTSTR	HEADER		;TYPE OUT COLUMN HEADER
	MOV	#7,R0		;WIDTH OF PRINT FIELD-1
	MOV	#2,R1		;2 DECIMAL DIGITS
	JSR	PC,FORMAT
	MOV	#8.,R0
	MOV	#2,R1
REDLP: 	MOV	#WLST,R0	;POINT TO WHERE COEF. LIST
	MOV	#DEVICE,R1	;POINT TO DEVICE BLOCK
	JSR	PC,WHERE	;READ THE CURRENT JOINT ANGLES
	TST	R0		;CHECK FOR ERRORS
	BEQ	GOODW		;BRANCH IF OK
	MOV	R0,-(SP)	;SAVE ERROR #
	MOV	#BADW,R0	;ELSE TYPE ERROR MESSAGE
	CLR	MASLC5		;AND INDICATE TO PDP10 THAT WE ARE DEAD
	JMP	TYPERR
GOODW:	OUTSTR	LINE2		;POSITION CURSOR AT 2ND LINE
	MOV	#THPTR,R2	;GET POINTERS JOINT READINGS
	ADD	WSWCH,R2	;ADD IN OFFSET
	MOV	#7,R3		;7 JOINTS 
JTCLP:	LDF 	@(R2)+,AC0	;PRINT OUT JOINT ANGLES
	NUMOUT
	SPACE
 	SOB	R3,JTCLP
	CRLF
	OUTSTR	CLRLIN		;CLEAR LINE FOR POT READINGS
	MOV	#POTPTR,R1	;PRINT OUT POT READINGS (BASE 10)
	ADD	WSWCH,R1	;ADD OFFSET
	MOV	#7,R3		;7 JOINTS 
NEXTP:	MOV	@(R1)+,R0
	MOV	R1,-(SP)
	JSR 	PC,TYPOCT	;PRINT OUT POT READING
	TAB
	MOV	(SP)+,R1
	SOB	R3,NEXTP
	CRLF
   .IFNZ TRANS
	MOV	#TRNADR,R0	;GET CURRENT TRANSFORM IN HERE
	MOV	#THPTR,R1	;POINT TO THE CURRENT JOINT ANGLES
	MOV	#BLUARM,R2	;ASSUME SOLUTION FOR BLUE ARM
	TST	WSWCH		;≠0 → BLUE ARM
	BNE	1$
	MOV	#YELARM,R2	;USE SOLN FOR YELLOW ARM
1$:	JSR	PC,UPDATE	;COMPUTE THE CURRENT TRANSFORM
	MOV	#TRNADR,R0	;COMPUTE THE EULER ANGLES
	MOV	#EANGLE,R1	;PUT THEM IN HERE
	JSR	PC,EULER
	OUTSTR	LINE5
	MOV	#6,R3
	MOV	#EANGLE,R1
EANGC:	LDF	(R1)+,AC0
	NUMOUT
	SPACE
	SOB	R3,EANGC
	MOV	#THPTR,R0	;TRANSFER THE JOINT ANGLES TO PDP10 
	ADD	WSWCH,R0
	MOV	#JTSADR,R1	;COMMUNICATIONS TABLE
	MOV	#7,R2		;6 JOINTS
RHTLP:	LDF	@(R0)+,AC0	
	STF	AC0,(R1)+
	SOB	R2,RHTLP
	MOV	#MASTR5,MASLC5	;INDICATE TO PDP10 THAT "WHERE" IS WORKING
   .ENDC
	JSR	PC,KBDCHK	;SETS CARRY BIT IF KBD HIT
	BCS	2$
	JMP	REDLP
2$:	CLR	MASLC5		;INDICATE "WHERE" NOT ACTIVE
	JSR	PC,RSTFOR
	BPT

;ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS

REF:	OUTSTR	COM5		;TYPE OUT THE CALIB. REFERENCE READING
	MOV	REFER1,R0	;GET REF. READING
	JSR 	PC,TYPNUM
	MOVB	#40,R0		;PUT IN A SPACE
	JSR	PC,TYPCHR
	MOV	REFER2,R0	;GET SECOND REF. READING
	JSR 	PC,TYPNUM
	CRLF
	CLR	DEVICE		;DON'T READ ANY TACHS
REFLP:	MOV	#DEVICE,R1	;READ THE REF. POWER SUPPLY VOLTAGE
	JSR	PC,SETREF
	CR
	TAB
	TAB			
	MOV	#15,R0
	JSR	PC,TYPCHR
	MOV	REFVT1,R0	;GET REFERENCE READINGS AND CONVERT TO ASC
	JSR	PC,TYPNUM
	MOVB	#40,R0		;PUT IN A SPACE
	JSR	PC,TYPCHR
	MOV	REFVT2,R0	;GET THE SECOND READING
	JSR	PC,TYPNUM
	JSR	PC,KBDCHK	;SETS CARRY BIT IF KBD HIT
	BCC	REFLP
;	CLRB	KBIR
	CRLF
	BPT

TYPNUM: MOV	R1,-(SP)
	MOV	R0,-(SP)
	BGE	1$
	NEG	(SP)
	MOV	#'-,R0
	JSR	PC,TYPCHR
1$:	MOV	(SP)+,R0
	JSR	PC,TYPDEC
	MOV	(SP)+,R1
	RTS	PC
;ROUTINE TO CHECK THE TOUCH SENSOR ON MONITOR SUBROUTINES 

;TTOUCH:	EVMAK			;CREATE A EVENT TO WAIT ON
;	MOV	(SP),R0		;GET THE EVENT NUMBER
;	MOV	SENSOR,R1	;GET THE SENSOR NUMBER AND TRIGGER STATE
;	JSR	PC,TOUCH	;INITIATE TOUCH SENSOR ON MONITOR
;	EVWAIT	(SP)		;WAIT FOR EVENT TO BE TRIGGERED
;	EVKIL			;GET RID OF THE EVENT
;	OUTSTR	EVMES		;TELL EVERYONE THE EVENT HAS BEEN TRIGGERED
;	BPT			;EXIT TO THE DEBUGGER

TTOUCH: mov 	#2,tctr		;two events to wait for

	evmak			;
	mov	(sp),ev1sav	;save event
	mov	(SP),R0	
	mov	#400,R1		;trigger w/ 1st sensor on
	jsr	pc,touch	;queue touch event
	fork	ba1ptr,#touch1,#usrdm

	evmak
	mov	(sp),ev2sav	;save event
	mov	(sp),r0
	mov	#401,r1		;trigger w/ 2nd sensor on
	jsr	pc,touch	;queue touch event
	fork	ba2ptr,#touch2,#usrdm

1$:	sleep	#50.
	tst	tctr
	bgt	1$
	evkil
	evkil
	bpt



touch1:	evwait	ev1sav
	outstr  evmes1
	dec	tctr
	dismis

touch2:	evwait	ev2sav
	outstr  evmes2
	dec	tctr
	dismis

data

ev1sav:	.word	0
ba1ptr: ba1pdb
ba1pdb:	PDBLK	5,20,,bazdat

ev2sav:	.word	0
ba2ptr: ba2pdb
ba2pdb:	PDBLK	5,20,,bazdat

tctr:	.word 	0
bazdat:	.word	0

evmes1:	.asciz /1st event signaledπ
/
evmes2:	.asciz /2nd event signaledπ
/

.even
;CENTER FUNCTION TEST ROUTINE
code
C:	MOV	#DEVICE,R1	;INITIALIZE BLUE ARM TO RUN
	JSR	PC,INTARM
	TST	R0		;CHECK FOR ERRORS
	BEQ	.+6		;SKIP IF NO ERRORS
	JMP	BADINT		;BRANCH IF INITIALIZATION ERROR
        MOV	#BARM+BHAND,DLST	;CENTER BLUE ARM
        MOV	#DLST,R0	;POINT TO COEFFICIENTS
	MOV	#DEVICE,R1	;POINT TO DEVICE BLOCK
	JSR	PC,CENTER
	OUTSTR	COM6		;TYPE OUT STATUS WORD
	JSR	PC,TYPOCT	
	CRLF
	MOV	DEVICE,R2	;GET THE NUMBER OF  SERVOS ATTACHED
	BEQ	ENDS5		;BRANCH IF NONE ATTACHED
	OUTSTR	COM7		;ELSE TYPE OUT THE DEVICE BLOCK CONTENTS
	MOV	#DEVICE+4,R3	;START WITH THIS WORD
DEVLP2:	MOV	(R3)+,R0
	JSR	PC,TYPOCT
	CRLF
	SOB	R2,DEVLP2
ENDS5:	BPT			;EXIT TO THE DEBUGGER

;END OF CENTER AND TOUCH SENSOR TESTS
;TACHOMETER CALIBRATION ROUTINE
; TO USE SET VSCALE TO SOME NON-ZERO VALUE
;	 RUN PROG WITHOUT MOVING JOINT AND ADJUST TACHZ0 UNTIL ESTIMATE IS 0
;	REPEAT FOLLOWING UNTIL ACTUAL = ESTIMATE
;	 START PROGRAM  AND SHOVE JOINT ONE DIRECTION WHILE BRAKE IS OFF
;	 DIVIDE ESTIMATED BY ACTUAL READINGS AND USE TO MODIFY VSCALE

CODE
TCALIB:	MOV 	TPTR,DAT
	CLRF	AC2		;CLEAR VELOCITY INTEGRAL
	MOV	#60.,R4		;TEST FOR 60 TICKS = 1 SEC
	JSR	PC,ANGLES	;GET INITIAL NGLE
	STF	AC0,AC4		;SAVE IT
	OUTSTR	TOPCLR		;CLEAR SCREEN
	MOV	#41,DR11S	;SET BRAKE MODE AND INT ENB B
	MOV	DACINF(DAT),DR11O   ;CLEAR BRAKE AND ENABLE INT B
	BIC	#7777,DACCHN(DAT) ;MAKE SURE DRIVE VALUE WILL BE 0

1$:	MOV	#40,DR11S	;PREPARE TO WRITE TO DAC
	MOV	DACCHN(DAT),DR11O	;SEND 0 TO DAC
	MOV	#42,DR11S	;PREPARE TO READ TACH
	MOVB	TACCHN(DAT),DR11O    ;START CONVERTING TACH READING
2$:	TSTB	DR11S
	BPL	2$
	MOV	DR11I,AC
	SUB	#3777,AC	;OFFSET SO 0=0
   	SUB	TACHZ0(DAT),AC	;SUBTRACT ZERO VELOCITY TACH READINGS
	LDCIF	AC,AC1		;CONVERT TO FLOATING POINT
	MULF	VSCALE(DAT),AC1	;CONVERT FROM A/D UNITS TO DEGREES/TICK
	STF 	AC1,THD(DAT)	;SAVE VELOCITY
	ADDF	AC1,AC2		;INTEGRATE VELOCITY

	SLEEP	#16.		;SLEEP FOR 1/60 SEC = 1 TICK
	DEC	R4
	BGT	1$		;DO FOR 60 STEPS 

TCALDN:	JSR	PC,MOTSTP	;SHUT DOWN JOINT
	JSR	PC,ANGLES
	SUBF	AC4,AC0		;GET DELTA THETA
	DIVF	SIXTY,AC0	;GET ACTUAL DEGREES/TICK INTO AC0
	OUTSTR	ACTMSG
	NUMOUT			;PRINT IT
	CRLF
	DIVF	SIXTY,AC2	;GET ESTIMATED DEGREES/TICK
	LDF	AC2,AC0
	OUTSTR	ESMSG		;PRINT IT
	NUMOUT
	CRLF
	BPT
	JMP	TCALIB
 
DATA
TPTR:	.WORD	SRV01
SIXTY:	.FLT2	60.0
ACTMSG:	.ASCIZ /ACTUAL DEGREES/TICK: /
ESMSG:  .ASCIZ /ESTIMATED DEGREES/TICK: /

.EVEN	


;END OF TCALIB
;ROUTINE TO TEST READ BASE VALUES FROM WRIST

CODE
RB:	OUTSTR	TOPCLR		;GO TO TOP OF VTO5 AND CLEAR IT
	OUTSTR	WMG1
2$:	MOV	#SAVBAS,R0	;WHERE TO PUT BASE READINGS
	JSR	PC,SETBAS	;GET 'EM 
	OUTSTR  LINE2		;POSITION CURSOR AT SECOND LINE
	MOV	#SAVBAS,R1	;WHERE THE READINGS ARE
	MOV	#8.,R2		;CONVERT 8. READINGS

1$:	MOV	(R1)+,R0	;PUT READING IN R0
	JSR	PC,TYPNUM	;CONVERT READING TO ASCII BASE 10
	SPACE
	SPACE
	CRLF
	SOB	R2,1$		;REPEAT UNTIL DONE WITH 8 READINGS

	SLEEP	#50.		;SLEEP FOR 50. MSEC
        TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	2$
	CLRB	KBIR
	CRLF
	BPT
;ROUTINE TO READ AND RESOLVE WRIST FORCES

RR:	OUTSTR	TOPCLR		;CLEAR SCREEN
	OUTSTR	FRCHDR		;PRINT HEADER
2$:	MOV 	#0,R0		;USE STANDARD CALIBRATION MATRIX
	MOV	#FRC,R1
	JSR	PC,WRIST	;READ WRIST AND RESOLVE IT
	MOV 	#FRC,R1		;CONVERT FORCES TO ASCII
	MOV	#6,R2
	OUTSTR	LINE2
1$:	LDF	(R1)+,AC0
	NUMOUT
	SPACE
	SOB	R2,1$

	SLEEP	#50.		;SLEEP FOR 50. MSEC
        TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	2$
	CLRB	KBIR
	CRLF
	BPT


;DATA FOR WRIST

DATA
FRC:	.BLKW	12.		;STORAGE FOR FORCES
SAVBAS:	.BLKW   8.		;BASE READING STORAGE AREA
;ROUTINES TO READ A/D CHANNELS
CODE
RCHAN:	clr 	blue		;0→blue   1→yellow
	OUTSTR	TOPCLR
	outstr  select
	jsr	pc,getnum
	mov	r0,blue
	OUTSTR  COM8
	JSR	PC,GETNUM
	BIC	#177700,R0
	MOV	R0,RADCHN	;SAVE A/D CHAN TO READ
	tst	blue
	beq	2$
	outstr	sayyel
	mov	#42,dr11s	;set read mode in yellow interface
	br	1$
2$:	outstr  sayblu
1$:	tst	blue
	bne	3$
	MOVB	RADCHN,ADCCHN   ;REQUEST ADC CONVERSION FROM BLUE INTERFACE
	TSTB	ADCSR		;CONVERSION DONE?
	BPL	.-4
	MOV	ADCVAL,R0	;GET ADC READING FORM BLUE INTERFACE
	br	4$
3$:	movb	radchn,dr11o
	tstb	dr11s
	bpl	.-4
	mov	dr11i,r0
	add	delta,r0
4$:	OUTSTR	LINE5
	JSR	PC,TYPNUM	;OUTPUT IN BASE 10
	JSR	PC,KBDCHK
	BCC	1$
	BPT

;ROUTINE TO READ and print 2 a/d channels

TCHAN:
	MOV	#MAINL+400,R4
1$:	MOVB	CHAN0,ADCCHN	;REQUEST CHAN0 TO BE READ
2$:	TSTB	ADCSR
	BPL	 2$
	MOV	ADCVAL,-(SP)

	MOVB	CHAN2,ADCCHN   ;REQUEST ADC CONVERSION FROM BLUE INTERFACE
12$:	TSTB	ADCSR		;CONVERSION DONE?
	BPL	12$
	MOV	ADCVAL,-(SP)	;SAVE ADC READING FORM BLUE INTERFACE

	MOVB	CHAN1,ADCCHN   ;REQUEST ADC CONVERSION FROM BLUE INTERFACE
22$:	TSTB	ADCSR		;CONVERSION DONE?
	BPL	22$
	MOV	ADCVAL,-(SP)	;GET ADC READING FORM BLUE INTERFACE

	MOVB	CHAN0,ADCCHN	;REQUEST 3RD CHANNEL TO BE READ
32$:	TSTB	ADCSR
	BPL	32$
	MOV	ADCVAL,R0

	MOV	R0,(R4)+	;SAVE AND PRINT CHAN0 VALUE
	JSR	PC,TYPNUM	;OUTPUT IN BASE 10
	SPACE
	SPACE
	MOV	(SP)+,R0	;GET READING
	MOV	R0,(R4)+	;SAVE AND PRINT CHAN1 VALUE
	JSR	PC,TYPNUM	;OUTPUT IN BASE 10
	SPACE	
	SPACE
	MOV	(SP)+,R0	;GET FIRST READING
	MOV	R0,(R4)+	;SAVE AND PRINT CHAN2 VALUE
	JSR	PC,TYPNUM	;OUTPUT IN BASE 10
	SPACE	
	SPACE
	MOV	(SP)+,R0	;GET FIRST READING
	MOV	R0,(R4)+	;SAVE AND PRINT CHAN0 VALUE AGAIN
	JSR	PC,TYPNUM	;OUTPUT IN BASE 10
	CRLF
	
	JSR	PC,KBDCHK	;SETS CARRY BIT IF KBD HIT
	BCC	1$
	BPT
DATA
CHAN2: 	.BYTE   41
CHAN1:	.BYTE	40
CHAN0:	.BYTE	21		;POTCHAN JOINT 1
.EVEN
DELTA:	.WORD	0

;ROUTINE TO CHECK IF KBD HIT
CODE
KBDCHK:	CLC			;ASSUME KBD NOT HIT
	MFPD	OUTSW		;CHECK VT05 OR TERM
	TST	(SP)+
	BEQ	1$		;BR IF TERMINAL MODE

	TSTB	KBIS		;ELSE CHECK VT05
	BEQ	14$
	CLRB	KBIR		;ENABLE INPUT
	BR	13$	
	RTS	PC

1$:	MFPD	IREG		;CHECK IF KBD HIT
	TST	(SP)+
	BNE	12$
	RTS	PC

12$:	CLR	-(SP)		;ENABLE INPUT
	MTPD	IREG
13$:	SEC			;SIGNAL KBD HIT
14$:	RTS	PC

;ROUTINE TO READ INTEGER FROM KBD

GETNUM:	MOV	R1,-(SP)
3$:	MOV	#IOBUF,R0
	JSR	PC,INSTR
	MOV	#IOBUF,R0
	JSR	PC,RELSCN
	TST	R1
	BNE	1$
	STCFI	AC0,R0
	MOV	(SP)+,R1
	RTS PC
1$:	OUTSTR	inmsg
	BR	3$

DATA
select:	.asciz 	/blue or yellow interface? (0:1) /
sayyel:	.asciz	/yellow interface/
sayblu: .asciz	/blue interface/
inmsg:	.ASCIZ/ INPUT AN INTEGER /
.EVEN
blue:	.word	0
RADCHN:	.WORD	0
CODE
;DRIVE COEFFICIENT DATA LIST
.EVEN
DATA
.EVEN

DLST:	0       ;JOINT SERVO BIT
	0
	0	;NO SPECIAL MODES
	0	;NO WOBBLE
	40.	;RELATIVE SEGMENT POINTER
DTIME:	160.
	0	;NO ASSOCIATED TRANS
	0	;NO RUN CODE
DA0:	.WORD	0,0
	.WORD	0,0
	.WORD	0,0
DA3:	.WORD	41040,0		;10.0
DA4:	.WORD 	141160,0	;-15.0
DA5:	.WORD	40700,0		;6.0
SEGCI:	.BLKW	2		;CI
SEGCII:	.BLKW	2		;CII
	0	;NO OTHER SEGMENTS

;WHERE COEFFICIENT DATA LIST

WLST:	774     ;BARM JOINT SERVO BITS 
	0

;FLOATING POINT CONSTANTS

C10:	.WORD	41040,0		;10.0
CM15:	.WORD	141160,0	;-15.0
C6:	.WORD	40700,0		;6.0

;TABLES FOR COMPUTE DYNAMIC COEFFICIENTS

CIPTR:	CILST
	CILST+4
	CILST+10
	CILST+14
	CILST+20
	CILST+24
	CIILST
	CIILST+4
	CIILST+10
	CIILST+14
	CIILST+20
	CIILST+24
CILST:	.BLKW	12.
CIILST:	.BLKW	12.
;LOCAL STORAGE AREA

DEVICE:	.BLKW	32.	;DEVICE BLOCK
IOBUF:	.BLKW	400.
EANGLE:	.BLKW	12.
SENSOR:	.BYTE	0,1	;BLUE ARM TOUCH SENSOR, TRIGGER ON ON CONDITION
WSWCH:	.WORD	0	;WHERE SWITCH 0→BARM 1→YARM

;OUTPUT STRINGS

MSG1:	.ASCIZ	/POWER OFF OR PANIC BUTTON ON...
/
NSCR:	.ASCIZ	/LOOKING FOR THE SCREWDRIVER
/
COM8:	.ASCIZ	/INPUT CHANNEL NUMBER (DECIMAL): /

SPACES:	.BYTE	40,40,11,0
TOPCLR:	.BYTE	35,35,37,37,37,10,10,10,0
TOP:	.BYTE	35,10,10,10,0
LINE2:	.BYTE	35,10,10,10,12,12,15,0
LINE5:	.BYTE	35,10,10,10,15,12,12,12,12,12,40,40,40,40,40,40,40,40,15,0
CLRLIN: .BYTE	36,0
COM4:	.BYTE	15,32,10,10,10
   .IFNZ TRANS
	.BYTE	32,10,10,10,32,10,10,10
   .ENDC
	.BYTE	0
HEADER:	.BYTE	35,10,10,10
	.ASCII	/
   JT 1    JT 2    JT 3    JT 4    JT 5    JT 6    HAND    VISE

/
   .IFNZ TRANS
        .ASCII  /
     X       Y       Z       O       A       T
/
   .ENDC
	.BYTE	0
FRCHDR:	.ASCIZ	/
      FX         FY         FZ         MX         MY         MZ    
/
WMG1:	.ASCIZ	/BASE READINGS FROM WRIST SENSOR/
BADW:	.ASCIZ	/
SERVO ERROR WHILE ATTEMPTING TO DO A "WHERE", ERROR CODE = /
BADSTR:	.ASCIZ	/
ARM INITIALIZATION FAILED, ERROR CODE = /
COM5:	.ASCIZ	/
REF. POWER SUPPLY READINGS DURING BARM CALIBRATION = /
REFSTG:	.ASCII	/CURRENT REFERENCE READINGS = /
REFSG2:	.ASCII	/                /
	.BYTE	15,10,10,10,0
COM6:	.ASCIZ	/STATUS WORD = /
COM7:	.ASCIZ	/THE DEVICE BLOCK SERVO WORDS ARE AS FOLLOWS:
/
EVMES:	.ASCII	/TOUCH SENSOR EVENT TRIGGERED
/
	.BYTE	7,0
.EVEN

;MAIN PROGRAM PROCESSOR DESCRIPTOR BLOCK

MAINL:	PDBLK	1,100,FP

CODE
temp == %OFFSE	;Save the current offset
.OFFSET 0	;We want to use real physical addresses here for the kernel

	PUTLOC JOBDAT, MAINL
	PUTLOC JOBSA, DINIT
	PUTLOC JOBDM, USRDM

.OFFSET temp	;Restore Offset

.MSG LAST DIAG2 LOCATION

.END DIAG